﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Simulador_Quadrotor
{
    public class GainSchedulingControl : Controlador
    {
        #region Propiedades

        public GainScheduling gainScheduling = null;
        public PID PidRoll { get; set; }
        public PID PidPitch { get; set; }
        public PID PidYaw { get; set; }
        public PID PidAltura { get; set; }
        public PID PidX { get; set; }
        public PID PidY { get; set; }

        public double Tp { get; set; }

        #endregion


        #region Atributos

        double controlZ;
        double controlPitch;
        double controlRoll;
        double controlYaw;
        double controlX;
        double controlY;
        double tiempoTotal;
        ControlMotor controlMotor = null;

        #endregion

        #region Metodos


        public GainSchedulingControl(GainScheduling gainScheduling)
        {
            this.gainScheduling = gainScheduling;
            this.tiempoTotal = 0;
            this.controlMotor = new ControlMotor();
            this.Tp = gainScheduling.GetTp();
        }

        public override double[] Controla(Estado estadoActual, Estado estadoFinal)
        {
            //cargamso el PID que mejor se ajuste
            if (PidAltura == null)
            {
                this.CargaPID(estadoActual.Z, estadoFinal.Z);
            }
            //controlamos la x
            controlX = this.PidX.Control(estadoActual, estadoFinal, estadoFinal.X - estadoActual.X);
            controlX = controlMotor.Saturar(controlX, 0.1, -0.1);
            //controlamos la Y
            controlY = this.PidY.Control(estadoActual, estadoFinal, estadoFinal.Y - estadoActual.Y);
            controlY = controlMotor.Saturar(controlY, 0.1, -0.1);
            //controlamos la altura
            controlZ = this.PidAltura.Control(estadoActual, estadoFinal, estadoFinal.Z - estadoActual.Z);
            //controlamos el Roll
            controlRoll = this.PidRoll.Control(estadoActual, estadoFinal, controlY - estadoActual.phi);
            //controlamos el Pitch              
            controlPitch = this.PidPitch.Control(estadoActual, estadoFinal, (-controlX) - estadoActual.theta);

            //controlamos el Yaw
            controlYaw = this.PidYaw.Control(estadoActual, estadoFinal, estadoFinal.psi - estadoActual.psi);
            //controlamos las salidas de los rotores
            return this.controlMotor.Controlar(controlZ, controlRoll, controlPitch, controlYaw, estadoFinal.Z - estadoActual.Z);

        }
        /// <summary>
        /// Dependiendo del rango carga los datos del PID
        /// </summary>
        /// <param name="alturaActual"></param>
        /// <param name="alturaDeseada"></param>
        private void CargaPID(double alturaActual, double alturaDeseada)
        {
            int rango = 0;
            double diferencia=alturaDeseada - alturaActual;
            rango = (int)Math.Truncate(diferencia);
            ConfiguracionPurebaPID configuracionPurebaPID = this.gainScheduling.DamePID(rango);

            this.PidAltura = new PID();
            this.PidAltura.Kd = configuracionPurebaPID.Altura.kd;
            this.PidAltura.Ki = configuracionPurebaPID.Altura.ki;
            this.PidAltura.Kp = configuracionPurebaPID.Altura.kp;
            this.PidAltura.Tp = configuracionPurebaPID.TPaso;

            this.PidX = new PID();
            this.PidX.Kd = configuracionPurebaPID.X.kd;
            this.PidX.Ki = configuracionPurebaPID.X.ki;
            this.PidX.Kp = configuracionPurebaPID.X.kp;
            this.PidX.Tp = configuracionPurebaPID.TPaso;

            this.PidY = new PID();
            this.PidY.Kd = configuracionPurebaPID.Y.kd;
            this.PidY.Ki = configuracionPurebaPID.Y.ki;
            this.PidY.Kp = configuracionPurebaPID.Y.kp;
            this.PidY.Tp = configuracionPurebaPID.TPaso;

            this.PidYaw = new PID();
            this.PidYaw.Kd = configuracionPurebaPID.Yaw.kd;
            this.PidYaw.Ki = configuracionPurebaPID.Yaw.ki;
            this.PidYaw.Kp = configuracionPurebaPID.Yaw.kp;
            this.PidYaw.Tp = configuracionPurebaPID.TPaso;

            this.PidRoll = new PID();
            this.PidRoll.Kd = configuracionPurebaPID.Roll.kd;
            this.PidRoll.Ki = configuracionPurebaPID.Roll.ki;
            this.PidRoll.Kp = configuracionPurebaPID.Roll.kp;
            this.PidRoll.Tp = configuracionPurebaPID.TPaso;

            this.PidPitch = new PID();
            this.PidPitch.Kd = configuracionPurebaPID.Pitch.kd;
            this.PidPitch.Ki = configuracionPurebaPID.Pitch.ki;
            this.PidPitch.Kp = configuracionPurebaPID.Pitch.kp;
            this.PidPitch.Tp = configuracionPurebaPID.TPaso;

            this.Tp = configuracionPurebaPID.TPaso;
        }
        #endregion
    }
}
